﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;

namespace SharpSoft.Maths
{
    /// <summary>
    /// 卡尔曼滤波算法（一维）
    /// </summary>
    public class KalmanFilter1
    {
        Kalman2D xk = new Kalman2D();

        public KalmanFilter1()
        {

        }
        /// <summary>
        /// 初始值
        /// </summary>
        public float InitValue { get; set; } = float.MinValue;
        /// <summary>
        /// 初始速度
        /// </summary>
        public float InitVelocity { get; set; } = 0f;
        /// <summary>
        /// 测算协方差
        /// </summary>
        public float MeasurementCovariance { get; set; } = 5f;
        /// <summary>
        /// 初始方差
        /// </summary>
        public float InitialVariance { get; set; } = 10f;
        /// <summary>
        /// 初始位置
        /// </summary>
        public float InitialValue { get; set; }
        /// <summary>
        /// 重置
        /// </summary>
        /// <param name="initPoint"></param>
        public void Reset()
        {
            xk.Reset(InitValue
                , InitVelocity
                , MeasurementCovariance
                , InitialVariance
                , InitialValue
                );
        }
        float? lastValue = null;
        double lastTime = 0;
        /// <summary>
        /// 更新数据并输出当前滤波值
        /// </summary>
        /// <param name="value"></param>
        /// <param name="time"></param>
        /// <returns></returns>
        public float Update(float value, double time)
        {
            double vx = 0d;
            double timespan = time - lastTime;
            if (lastValue.HasValue)
            {//计算速度
                vx = (value - lastValue.Value) / timespan;
            }
            lastValue = value;
            lastTime = time;
            var kx = xk.Update(value, vx, 1);
            return Convert.ToSingle(kx);
        }

    }
}
